#include <math.h>

#define M_PI 3.14159265358979323846f


/**
 * @brief 计算平方根的倒数
 * @param x 输入值
 * @return 平方根的倒数
 * @note
 * 1. 快速
 * 2. 准确
 * 3. 逆天
 */
float invSqrt(float x) 
{
    float halfx = 0.5f * x;
    float y = x;
    long i = *(long*)&y;  // evil floating point bit level hacking
    i = 0x5f3759df - (i >> 1);  // what the fuck?  
    y = *(float*)&i;
    y = y * (1.5f - (halfx * y * y));  // 1st iteration
    return y;
}

/**
 * @brief 计算自然对数的底2
 * @param x 输入值
 * @return 自然对数的底2
 */
float fastLog2(float x) {
    int i = *(int*)&x;
    return (i - 0x3f800000) * 1.1920928955078125e-7f; // 1 / (2^23)
}



/**
 * @brief 计算正弦值
 * @param x 输入值
 * @return 正弦值
 */
float fastSin(float x) {
    const float B = 4.0f / M_PI;
    const float C = -4.0f / (M_PI * M_PI);
    float y = B * x + C * x * fabs(x);
    return y;
}


/**
 * @brief 快速计算 3x3 或 4x4 矩阵的逆矩阵
 * @param m 输入矩阵
 * @param inv 输出逆矩阵
 */
void fastInverse3x3(float *m, float *inv) {
    float det = m[0] * (m[4] * m[8] - m[5] * m[7]) -
                m[1] * (m[3] * m[8] - m[5] * m[6]) +
                m[2] * (m[3] * m[7] - m[4] * m[6]);
    float invDet = 1.0f / det;
    inv[0] = (m[4] * m[8] - m[5] * m[7]) * invDet;
    inv[1] = (m[2] * m[7] - m[1] * m[8]) * invDet;
    inv[2] = (m[1] * m[5] - m[2] * m[4]) * invDet;
    inv[3] = (m[5] * m[6] - m[3] * m[8]) * invDet;
    inv[4] = (m[0] * m[8] - m[2] * m[6]) * invDet;
    inv[5] = (m[2] * m[3] - m[0] * m[5]) * invDet;
    inv[6] = (m[3] * m[7] - m[4] * m[6]) * invDet;
    inv[7] = (m[1] * m[6] - m[0] * m[7]) * invDet;
    inv[8] = (m[0] * m[4] - m[1] * m[3]) * invDet;
}

/**
 * @brief 快速计算 4x4 矩阵的逆矩阵
 * @param m 输入矩阵
 * @param inv 输出逆矩阵
 */
void fastInverse4x4(float *m, float *inv) {
    float det = 
        m[0] * (m[5] * (m[10] * m[15] - m[11] * m[14]) - 
                m[6] * (m[9] * m[15] - m[11] * m[13]) + 
                m[7] * (m[9] * m[14] - m[10] * m[13])) -
        m[1] * (m[4] * (m[10] * m[15] - m[11] * m[14]) - 
                m[6] * (m[8] * m[15] - m[11] * m[12]) + 
                m[7] * (m[8] * m[14] - m[10] * m[12])) +
        m[2] * (m[4] * (m[9] * m[15] - m[11] * m[13]) - 
                m[5] * (m[8] * m[15] - m[11] * m[12]) + 
                m[7] * (m[8] * m[13] - m[9] * m[12])) -
        m[3] * (m[4] * (m[9] * m[14] - m[10] * m[13]) - 
                m[5] * (m[8] * m[14] - m[10] * m[12]) + 
                m[6] * (m[8] * m[13] - m[9] * m[12]));

    float invDet = 1.0f / det;

    inv[0] = (m[5] * (m[10] * m[15] - m[11] * m[14]) - 
              m[6] * (m[9] * m[15] - m[11] * m[13]) + 
              m[7] * (m[9] * m[14] - m[10] * m[13])) * invDet;
    inv[1] = (m[1] * (m[11] * m[14] - m[10] * m[15]) - 
              m[2] * (m[7] * m[14] - m[6] * m[15]) + 
              m[3] * (m[6] * m[13] - m[7] * m[12])) * invDet;
    inv[2] = (m[1] * (m[6] * m[15] - m[7] * m[14]) - 
              m[2] * (m[5] * m[15] - m[7] * m[13]) + 
              m[3] * (m[5] * m[14] - m[6] * m[13])) * invDet;
    inv[3] = (m[1] * (m[7] * m[10] - m[6] * m[11]) - 
              m[2] * (m[7] * m[9] - m[5] * m[11]) + 
              m[3] * (m[6] * m[9] - m[5] * m[10])) * invDet;

    inv[4] = (m[4] * (m[11] * m[14] - m[10] * m[15]) - 
              m[6] * (m[8] * m[15] - m[11] * m[12]) + 
              m[7] * (m[8] * m[14] - m[10] * m[12])) * invDet;
    inv[5] = (m[0] * (m[10] * m[15] - m[11] * m[14]) - 
              m[2] * (m[8] * m[15] - m[11] * m[12]) + 
              m[3] * (m[8] * m[14] - m[10] * m[12])) * invDet;
    inv[6] = (m[0] * (m[11] * m[14] - m[10] * m[15]) - 
              m[1] * (m[8] * m[15] - m[11] * m[12]) + 
              m[3] * (m[8] * m[14] - m[10] * m[12])) * invDet;
    inv[7] = (m[0] * (m[10] * m[15] - m[11] * m[14]) - 
              m[1] * (m[8] * m[15] - m[11] * m[12]) + 
              m[2] * (m[8] * m[14] - m[10] * m[12])) * invDet;

    inv[8] = (m[4] * (m[9] * m[15] - m[11] * m[13]) - 
              m[5] * (m[8] * m[15] - m[11] * m[12]) + 
              m[7] * (m[8] * m[13] - m[9] * m[12])) * invDet;
    inv[9] = (m[0] * (m[11] * m[13] - m[9] * m[15]) - 
              m[1] * (m[8] * m[15] - m[11] * m[12]) + 
              m[3] * (m[8] * m[13] - m[9] * m[12])) * invDet;
    inv[10] = (m[0] * (m[9] * m[15] - m[11] * m[13]) - 
               m[1] * (m[8] * m[15] - m[11] * m[12]) + 
               m[3] * (m[8] * m[13] - m[9] * m[12])) * invDet;
    inv[11] = (m[0] * (m[11] * m[13] - m[9] * m[15]) - 
               m[1] * (m[8] * m[15] - m[11] * m[12]) + 
               m[2] * (m[8] * m[13] - m[9] * m[12])) * invDet;

    inv[12] = (m[4] * (m[10] * m[13] - m[9] * m[14]) - 
               m[5] * (m[8] * m[14] - m[10] * m[12]) + 
               m[6] * (m[8] * m[13] - m[9] * m[12])) * invDet;
    inv[13] = (m[0] * (m[9] * m[14] - m[10] * m[13]) - 
               m[1] * (m[8] * m[14] - m[10] * m[12]) + 
               m[2] * (m[8] * m[13] - m[9] * m[12])) * invDet;
    inv[14] = (m[0] * (m[10] * m[13] - m[9] * m[14]) - 
               m[1] * (m[8] * m[14] - m[10] * m[12]) + 
               m[2] * (m[8] * m[13] - m[9] * m[12])) * invDet;
    inv[15] = (m[0] * (m[9] * m[14] - m[10] * m[13]) - 
               m[1] * (m[8] * m[14] - m[10] * m[12]) + 
               m[2] * (m[8] * m[13] - m[9] * m[12])) * invDet;
}
